﻿#include <algorithm>
#include <gtest/gtest.h>
#include <iostream>
#include <vector>

#include <learn/alg/alg_interpolator.h>
#include <learn/alg/alg_projection.h>
#include <learn/utils/timer.h>

using namespace xi;
using namespace xi::alg;

class TestProjection : public ::testing::Test
{
  public:
    // 初始化函数，其中初始化要测试的数据
    void SetUp() override
    {
        generator = std::mt19937(std::random_device{}()); // 使用 time(nullptr) 作为随机数生成器的种子
        uniform = std::uniform_real_distribution<double>(0.0, 10.0); // 在 [0,1] 区间内的均匀分布

        // 初始化曲线
        {
            int N = 100;
            std::vector<Vec3> points;
            for (int i = 0; i < N; i++)
                points.push_back(Vec3{uniform(generator), uniform(generator), uniform(generator)});

            Alg_CubicBSpline_Interpolator<3> interpolator;
            curve = interpolator.interpolate(points, interpolator.Natural, interpolator.Natural, Vec3{}, Vec3{});
        }

        // 初始化曲面
        {
            auto f = [](double u, double v) -> Vec3 { return Vec3{u, v, sin(u + v) * cos(u - v)}; };
            // 20 x 30 个采样点
            int N = 10;
            int M = 15;
            std::vector<std::vector<Vec3>> points(N, std::vector<Vec3>(M));
            for (int i = 0; i < N; i++)
                for (int j = 0; j < M; j++)
                    points[i][j] = f(10.0 / (N - 1) * i, 10.0 / (M - 1) * j);
            surface = Alg_CubicBSpline_Interpolator<3>::interpolate(points);
        }
    }

    // 析构函数，用来销毁测试数据，这里不需要
    // void TearDown() override {}

    std::shared_ptr<Geom_BSplineCurve<3>> curve;
    std::shared_ptr<Geom_BSplineSurface<3>> surface;

    std::mt19937 generator;
    std::uniform_real_distribution<double> uniform;
};

// 测试点到曲线的投影函数
TEST_F(TestProjection, Point2Curve)
{
    int M = 100;
    int valid = 0;

    TIMER_START(Near)

    for (int i = 0; i < M; i++)
    {
        Vec3 P{uniform(generator), uniform(generator), uniform(generator)};
        double t = 0.5;

        // 测试最近投影（不稳定，需要接近最近点）
        t = Alg_Projection<3>::nearest(curve, P, t, Numeric::distance);
        auto [Q, V1] = curve->d1(t);
        auto cosine = dot(V1.normalize(), (P - Q).normalize());
        auto distance = (P - Q).modulus();

        if (std::abs(cosine) < Numeric::distance)
            valid++;
        // EXPECT_LE(cosine, Numeric::distance);
    }

    TIMER_END(Near)

    std::cout << 1.0 * valid / M << std::endl;
    // EXPECT_GE(1.0 * valid / M, 0.90);

    valid = 0;

    TIMER_START(Ortho1)

    for (int i = 0; i < M; i++)
    {
        Vec3 P{uniform(generator), uniform(generator), uniform(generator)};
        double t = 0.5;

        // 测试 1 阶正交投影（不稳定，需要接近最近点）
        t = Alg_Projection<3>::orthogonal1(curve, P, t, Numeric::distance);
        auto [Q, V1] = curve->d1(t);
        auto cosine = dot(V1.normalize(), (P - Q).normalize());
        auto distance = (P - Q).modulus();

        if (std::abs(cosine) < Numeric::distance)
            valid++;
        // std::cout << distance << " " << cosine << std::endl;
        // EXPECT_LE(cosine, Numeric::distance);
    }

    TIMER_END(Ortho1)

    std::cout << 1.0 * valid / M << std::endl;
    // EXPECT_GE(1.0 * valid / M, 0.90);

    valid = 0;

    TIMER_START(Ortho2)

    for (int i = 0; i < M; i++)
    {
        Vec3 P{uniform(generator), uniform(generator), uniform(generator)};
        double t = 0.5;

        // 测试 2 阶正交投影（较稳定）
        t = Alg_Projection<3>::orthogonal2(curve, P, t, Numeric::distance);
        auto [Q, V1] = curve->d1(t);
        auto cosine = dot(V1.normalize(), (P - Q).normalize());
        auto distance = (P - Q).modulus();

        if (std::abs(cosine) < Numeric::distance)
            valid++;
        // std::cout << distance << " " << cosine << std::endl;
        // EXPECT_LE(cosine, Numeric::distance);
    }

    TIMER_END(Ortho2)

    std::cout << 1.0 * valid / M << std::endl;
    // EXPECT_GE(1.0 * valid / M, 0.95);
}

// 测试点到曲线的平行投影函数
TEST_F(TestProjection, Point2Curve_parallel)
{
    TIMER_START(curve_parallel)

    int M = 100;
    int valid = 0;

    std::uniform_real_distribution<double> tar(curve->first_param(), curve->last_param());
    for (int i = 0; i < M; i++)
    {
        Vec3 dir{uniform(generator), uniform(generator), uniform(generator)};
        double u = tar(generator);
        Vec3 P = curve->value(u) - dir;

        double t = 0.5;
        t = Alg_Projection<3>::parallel(curve, P, dir, t, 1e-2);
        auto Q = curve->value(t);
        auto cosine = cross(dir.normalize(), (P - Q).normalize()).modulus();

        // std::cout << cosine << " " << sin(degrees_to_radians(0.05)) << std::endl;
        if (std::abs(cosine) < 1e-2)
            valid++;
    }

    TIMER_END(curve_parallel)

    std::cout << 1.0 * valid / M << std::endl;
    // EXPECT_GE(1.0 * valid / M, 0.95);
}

// 测试点到曲面的投影函数
TEST_F(TestProjection, Point2Surface)
{
    auto [U1, U2, V1, V2] = surface->bounds();

    TIMER_START(Near)

    int M = 100;
    int valid = 0;
    for (int i = 0; i < M; i++)
    {
        Vec3 P{uniform(generator), uniform(generator), uniform(generator) / 5};
        double u = 0.5, v = 0.5;

        auto uv = Alg_Projection<3>::nearest(surface, P, u, v, Numeric::distance);
        auto [Q, DU, DV] = surface->d1(uv.x(), uv.y());
        auto angle = cross(cross(DU, DV).normalize(), (P - Q).normalize()).modulus();

        if (std::abs(angle) < Numeric::distance)
            valid++;
    }

    TIMER_END(Near)

    std::cout << 1.0 * valid / M << std::endl;
    // EXPECT_GE(1.0 * valid / M, 0.95);
    valid = 0;

    TIMER_START(surface_parallel)

    Vec3 O = surface->value(0.25, 0.75);
    for (int i = 0; i < M; i++)
    {
        Vec3 dir{uniform(generator), uniform(generator), uniform(generator)};
        auto P = O - dir;
        double u = 0.5, v = 0.5;

        auto uv = Alg_Projection<3>::parallel(surface, P, dir, u, v, Numeric::distance);
        auto Q = surface->value(uv.x(), uv.y());
        auto angle = cross(dir.normalize(), (P - Q).normalize()).modulus();

        if (std::abs(angle) < Numeric::distance)
            valid++;
    }

    TIMER_END(surface_parallel)

    std::cout << 1.0 * valid / M << std::endl;
    // EXPECT_GE(1.0 * valid / M, 0.95);
    valid = 0;

    TIMER_START(Ortho1)

    for (int i = 0; i < M; i++)
    {
        Vec3 P{uniform(generator), uniform(generator), uniform(generator) / 5};
        double u = 0.5, v = 0.5;

        auto uv = Alg_Projection<3>::orthogonal1(surface, P, u, v, Numeric::distance);
        auto [Q, DU, DV] = surface->d1(uv.x(), uv.y());
        auto angle = cross(cross(DU, DV).normalize(), (P - Q).normalize()).modulus();

        if (std::abs(angle) < Numeric::distance)
            valid++;
    }

    TIMER_END(Ortho1)

    std::cout << 1.0 * valid / M << std::endl;
    // EXPECT_GE(1.0 * valid / M, 0.95);
    valid = 0;

    TIMER_START(Ortho2)

    for (int i = 0; i < M; i++)
    {
        Vec3 P{uniform(generator), uniform(generator), uniform(generator) / 5};
        double u = 0.5, v = 0.5;

        auto uv = Alg_Projection<3>::orthogonal2(surface, P, u, v, Numeric::distance);
        auto [Q, DU, DV] = surface->d1(uv.x(), uv.y());
        auto angle = cross(cross(DU, DV).normalize(), (P - Q).normalize()).modulus();

        if (std::abs(angle) < Numeric::distance)
            valid++;
    }

    TIMER_END(Ortho2)

    std::cout << 1.0 * valid / M << std::endl;
    // EXPECT_GE(1.0 * valid / M, 0.95);
    valid = 0;

    TIMER_START(Torus)

    // {1.38115 0.277605 0.66348 }
    // Vec3 P{ 1.210816402930, 9.741359425732 ,1.293815732959 };
    // double u = 0.5, v = 0.5;
    // auto uv = torus<3>(surface, P, u, v, Numeric::distance);
    // auto [Q, DU, DV] = surface->d1(uv.x(), uv.y());
    // auto angle = ((DU % DV).normalize() % (P - Q).normalize()).modulus();

    for (int i = 0; i < M; i++)
    {
        Vec3 P{uniform(generator), uniform(generator), uniform(generator) / 5};
        double u = 0.5, v = 0.5;

        auto uv = Alg_Projection<3>::torus(surface, P, u, v, Numeric::distance);
        auto [Q, DU, DV] = surface->d1(uv.x(), uv.y());
        auto angle = cross(cross(DU, DV).normalize(), (P - Q).normalize()).modulus();

        // std::cout << i << " " << angle << std::endl;
        if (std::abs(angle) < Numeric::distance)
            valid++;
    }

    TIMER_END(Torus)

    std::cout << 1.0 * valid / M << std::endl;
    // EXPECT_GE(1.0 * valid / M, 0.95);
    valid = 0;
}

// // 测试曲线到曲面的投影函数
// TEST_F(TestProjection, Curve2Surface)
// {
//     double U1, U2, V1, V2;
//     surface->Bounds(U1, U2, V1, V2);

//     int valid = 0;
//     int T = 100;

//     TIMER_START(March)

//     std::vector<BPnt2> para;
//     ProjMat::March(curve, surface, 0, 1, T, XI_ABS_ACC, para);
//     double k = 1.0 / T;

//     TIMER_END(March)

//     for (int i = 0; i <= T; i++)
//     {
//         double t = k * i;
//         auto P = curve->Value(t);

//         Vec3 Q;
//         BVec DU, DV;
//         surface->D1(para[i][0], para[i][1], Q, DU, DV);
//         auto N = (DU % DV).Normalized();

//         double angle = (N % (P - Q)).Modulus();
//         auto distance = P.Distance(Q);

//         auto u = para[i][0];
//         auto v = para[i][1];
//         // if (distance < XI_ABS_ACC || fabs(angle) < XI_ABS_ACC || (fabs(u - U1) < XI_ABS_ACC * 2) ||
//         //     (fabs(u - U2) < XI_ABS_ACC * 2) || (fabs(v - V1) < XI_ABS_ACC * 2) || (fabs(v - V2) < XI_ABS_ACC *
//         //     2)) valid++;
//         if (distance < XI_ABS_ACC || fabs(angle) < XI_ABS_ACC)
//             valid++;
//     }

//     dbg(1.0 * valid / T);
//     EXPECT_TRUE(1.0 * valid / T > 0.5);
//     valid = 0;
//     para.clear();

//     TIMER_START(ODE)

//     ProjMat::ODE(curve, surface, 0, 1, T, XI_ABS_ACC, para);

//     TIMER_END(ODE)

//     for (int i = 0; i <= T; i++)
//     {
//         double t = k * i;
//         auto P = curve->Value(t);

//         Vec3 Q;
//         BVec DU, DV;
//         surface->D1(para[i][0], para[i][1], Q, DU, DV);
//         auto N = (DU % DV).Normalized();

//         double angle = (N % (P - Q)).Modulus();
//         auto distance = P.Distance(Q);

//         auto u = para[i][0];
//         auto v = para[i][1];
//         // if (distance < XI_ABS_ACC || fabs(angle) < XI_ABS_ACC || (fabs(u - U1) < XI_ABS_ACC * 2) ||
//         //     (fabs(u - U2) < XI_ABS_ACC * 2) || (fabs(v - V1) < XI_ABS_ACC * 2) || (fabs(v - V2) < XI_ABS_ACC *
//         //     2)) valid++;
//         if (distance < XI_ABS_ACC || fabs(angle) < XI_ABS_ACC)
//             valid++;
//     }

//     dbg(1.0 * valid / T);
//     EXPECT_TRUE(1.0 * valid / T > 0.7);
//     valid = 0;
// }